#!/usr/bin/python
import posefind_tools as pft;
import random as rnd;
import numpy as np;
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt

reload(pft);

def delta(d):
	return 2*d*rnd.random()-d;

def randomCenter(c0):
	d=5e-3;
	return [c0[0]+delta(d),c0[1]+delta(d),c0[2]+delta(d)];

def randomDir(Th0):
	d=1e-1;
	return [Th0[0]+delta(d),Th0[1]+delta(d),Th0[2]+delta(d)];

if __name__=="__main__":
	print "Starting";
	moons=pft.Moons();
	cam0=moons.getCamera(2.0);


	vx=[];vy=[];vz=[];
	wx=[];wy=[];wz=[];
	vrx=[];vry=[];vrz=[];
	for ux in [-20e-4,-10e-4,0.0,10e-4,20e-4]:
		for uy in [-20e-4,-10e-4,0.0,10e-4,20e-4]:
			U=[ux,uy];
			c0=cam0.getC();
			Th0=cam0.getTh();
			#y0=np.array([[ux],[uy],[-cam0.focal_length]]);
			y=cam0.camToReal(U);
			wx.append(y[0][0]);
			wy.append(y[1][0]);
			wz.append(y[2][0]);
			try:
				x0=cam0.camToFocal(U);
			except:
				print U;
			for i in range(100):
				c=randomCenter(c0);
				Th=randomDir(Th0)
				cam=pft.Camera(c,Th);	
				cam.setFocalLength(cam0.focal_length);
				try:
					print U;
					x=cam.camToFocal(U);
					n=np.linalg.norm(x-x0);
					vx.append(-n);
					vy.append(x0[1][0]);
					vz.append(x0[2][0]);
					vrx.append(x0[0][0]);
				except:
					print U;
	print "============================";
	print len(vx);
	fig = plt.figure();
	ax = fig.add_subplot(111, projection='3d');
	ax.plot(vx,vy,vz,"ro");
	ax.plot(wx,wy,wz,"go");
	ax.plot(vrx,vy,vz,"bo");
	moons.plotDish(ax);
	cam0.plotme(ax,0.2);
	plt.xlabel("x");
	plt.ylabel("y");
	fig.show();
	pause=raw_input("Pausa");


